// MIT License

// Copyright (c) 2019 Erin Catto

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "box2d/b2_block_allocator.h"
#include "box2d/b2_body.h"
#include "box2d/b2_distance_joint.h"
#include "box2d/b2_draw.h"
#include "box2d/b2_friction_joint.h"
#include "box2d/b2_gear_joint.h"
#include "box2d/b2_motor_joint.h"
#include "box2d/b2_mouse_joint.h"
#include "box2d/b2_prismatic_joint.h"
#include "box2d/b2_pulley_joint.h"
#include "box2d/b2_revolute_joint.h"
#include "box2d/b2_weld_joint.h"
#include "box2d/b2_wheel_joint.h"
#include "box2d/b2_world.h"

#include <new>

void b2LinearStiffness(float& stiffness, float& damping,
  float frequencyHertz, float dampingRatio,
  const b2Body* bodyA, const b2Body* bodyB)
{
  float massA = bodyA->GetMass();
  float massB = bodyB->GetMass();
  float mass;
  if (massA > 0.0f && massB > 0.0f)
  {
    mass = massA * massB / (massA + massB);
  }
  else if (massA > 0.0f)
  {
    mass = massA;
  }
  else
  {
    mass = massB;
  }

  float omega = 2.0f * b2_pi * frequencyHertz;
  stiffness = mass * omega * omega;
  damping = 2.0f * mass * dampingRatio * omega;
}

void b2AngularStiffness(float& stiffness, float& damping,
  float frequencyHertz, float dampingRatio,
  const b2Body* bodyA, const b2Body* bodyB)
{
  float IA = bodyA->GetInertia();
  float IB = bodyB->GetInertia();
  float I;
  if (IA > 0.0f && IB > 0.0f)
  {
    I = IA * IB / (IA + IB);
  }
  else if (IA > 0.0f)
  {
    I = IA;
  }
  else
  {
    I = IB;
  }

  float omega = 2.0f * b2_pi * frequencyHertz;
  stiffness = I * omega * omega;
  damping = 2.0f * I * dampingRatio * omega;
}

b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
{
  b2Joint* joint = nullptr;

  switch (def->type)
  {
  case e_distanceJoint:
    {
      void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
      joint = new (mem) b2DistanceJoint(static_cast<const b2DistanceJointDef*>(def));
    }
    break;

  case e_mouseJoint:
    {
      void* mem = allocator->Allocate(sizeof(b2MouseJoint));
      joint = new (mem) b2MouseJoint(static_cast<const b2MouseJointDef*>(def));
    }
    break;

  case e_prismaticJoint:
    {
      void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
      joint = new (mem) b2PrismaticJoint(static_cast<const b2PrismaticJointDef*>(def));
    }
    break;

  case e_revoluteJoint:
    {
      void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
      joint = new (mem) b2RevoluteJoint(static_cast<const b2RevoluteJointDef*>(def));
    }
    break;

  case e_pulleyJoint:
    {
      void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
      joint = new (mem) b2PulleyJoint(static_cast<const b2PulleyJointDef*>(def));
    }
    break;

  case e_gearJoint:
    {
      void* mem = allocator->Allocate(sizeof(b2GearJoint));
      joint = new (mem) b2GearJoint(static_cast<const b2GearJointDef*>(def));
    }
    break;

  case e_wheelJoint:
    {
      void* mem = allocator->Allocate(sizeof(b2WheelJoint));
      joint = new (mem) b2WheelJoint(static_cast<const b2WheelJointDef*>(def));
    }
    break;

  case e_weldJoint:
    {
      void* mem = allocator->Allocate(sizeof(b2WeldJoint));
      joint = new (mem) b2WeldJoint(static_cast<const b2WeldJointDef*>(def));
    }
    break;
        
  case e_frictionJoint:
    {
      void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
      joint = new (mem) b2FrictionJoint(static_cast<const b2FrictionJointDef*>(def));
    }
    break;

  case e_motorJoint:
    {
      void* mem = allocator->Allocate(sizeof(b2MotorJoint));
      joint = new (mem) b2MotorJoint(static_cast<const b2MotorJointDef*>(def));
    }
    break;

  default:
    b2Assert(false);
    break;
  }

  return joint;
}

void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
{
  joint->~b2Joint();
  switch (joint->m_type)
  {
  case e_distanceJoint:
    allocator->Free(joint, sizeof(b2DistanceJoint));
    break;

  case e_mouseJoint:
    allocator->Free(joint, sizeof(b2MouseJoint));
    break;

  case e_prismaticJoint:
    allocator->Free(joint, sizeof(b2PrismaticJoint));
    break;

  case e_revoluteJoint:
    allocator->Free(joint, sizeof(b2RevoluteJoint));
    break;

  case e_pulleyJoint:
    allocator->Free(joint, sizeof(b2PulleyJoint));
    break;

  case e_gearJoint:
    allocator->Free(joint, sizeof(b2GearJoint));
    break;

  case e_wheelJoint:
    allocator->Free(joint, sizeof(b2WheelJoint));
    break;
    
  case e_weldJoint:
    allocator->Free(joint, sizeof(b2WeldJoint));
    break;

  case e_frictionJoint:
    allocator->Free(joint, sizeof(b2FrictionJoint));
    break;

  case e_motorJoint:
    allocator->Free(joint, sizeof(b2MotorJoint));
    break;

  default:
    b2Assert(false);
    break;
  }
}

b2Joint::b2Joint(const b2JointDef* def)
{
  b2Assert(def->bodyA != def->bodyB);

  m_type = def->type;
  m_prev = nullptr;
  m_next = nullptr;
  m_bodyA = def->bodyA;
  m_bodyB = def->bodyB;
  m_index = 0;
  m_collideConnected = def->collideConnected;
  m_islandFlag = false;
  m_userData = def->userData;

  m_edgeA.joint = nullptr;
  m_edgeA.other = nullptr;
  m_edgeA.prev = nullptr;
  m_edgeA.next = nullptr;

  m_edgeB.joint = nullptr;
  m_edgeB.other = nullptr;
  m_edgeB.prev = nullptr;
  m_edgeB.next = nullptr;
}

bool b2Joint::IsEnabled() const
{
  return m_bodyA->IsEnabled() && m_bodyB->IsEnabled();
}

void b2Joint::Draw(b2Draw* draw) const
{
  const b2Transform& xf1 = m_bodyA->GetTransform();
  const b2Transform& xf2 = m_bodyB->GetTransform();
  b2Vec2 x1 = xf1.p;
  b2Vec2 x2 = xf2.p;
  b2Vec2 p1 = GetAnchorA();
  b2Vec2 p2 = GetAnchorB();

  b2Color color(0.5f, 0.8f, 0.8f);

  switch (m_type)
  {
  case e_distanceJoint:
    draw->DrawSegment(p1, p2, color);
    break;

  case e_pulleyJoint:
  {
    b2PulleyJoint* pulley = (b2PulleyJoint*)this;
    b2Vec2 s1 = pulley->GetGroundAnchorA();
    b2Vec2 s2 = pulley->GetGroundAnchorB();
    draw->DrawSegment(s1, p1, color);
    draw->DrawSegment(s2, p2, color);
    draw->DrawSegment(s1, s2, color);
  }
  break;

  case e_mouseJoint:
  {
    b2Color c;
    c.Set(0.0f, 1.0f, 0.0f);
    draw->DrawPoint(p1, 4.0f, c);
    draw->DrawPoint(p2, 4.0f, c);

    c.Set(0.8f, 0.8f, 0.8f);
    draw->DrawSegment(p1, p2, c);

  }
  break;

  default:
    draw->DrawSegment(x1, p1, color);
    draw->DrawSegment(p1, p2, color);
    draw->DrawSegment(x2, p2, color);
  }
}
